#include "bsp_pid.h"



//定义PID结构体
_PID_Typedef PID_Struct_Vertical;
_PID_Typedef PID_Struct_Velocity;
_PID_Typedef PID_Struct_Turn;

/**
  * @brief  PID结构体初始化。
  * @param  PID_Struct：PID结构体。
  * @retval 无。
  */
void PID_Struct_Init(_PID_Typedef *PID_Struct,unsigned char Channel)
{
  PID_Struct->PID_Channel_Val = Channel;
  PID_Struct->PID_Target_Val = 0;
  PID_Struct->PID_Actual_Val = 0;
  PID_Struct->PID_Err = 0;
  PID_Struct->PID_Err_Last = 0;
  PID_Struct->PID_Integral_Val = 0;
  PID_Struct->PID_P = 0;
  PID_Struct->PID_I = 0;
  PID_Struct->PID_D = 0;
  PID_Struct->LPF = 0;
  PID_Struct->PID_dt = 1;
}

/**
  * @brief  设置PID的目标值。
  * @param  PID_Struct：PID结构体。
  * @param  value：目标值。
  * @retval 无。
  */
void SET_PID_Target_Val(_PID_Typedef *PID_Struct, float value)
{
  PID_Struct->PID_Target_Val = value;
}

/**
  * @brief  获取PID的目标值。
  * @param  PID_Struct：PID结构体。
  * @retval 目标值。
  */
int GET_PID_Target_Val(_PID_Typedef *PID_Struct)
{
  return PID_Struct->PID_Target_Val;
}

/**
  * @brief  设置PID的比例、积分、微分系数。
  * @param  P_Value：比例系数 P
  * @param  I_Value：积分系数 I
  * @param  D_Value：微分系数 D
  * @retval 无。
  */
void SET_PID_Param(_PID_Typedef *PID_Struct, float P_Value, float I_Value, float D_Value)
{
  PID_Struct->PID_P = P_Value;
  PID_Struct->PID_I = I_Value;
  PID_Struct->PID_D = D_Value;
}

/**
  * @brief  PID调节代码实例
  * @param  None
  * @retval None
  */
float PID_Example(_PID_Typedef *PID_Struct)
{
  float output;     //PID处理结果。
  float derivative;   //PID微分。

  PID_Struct->PID_Err = PID_Struct->PID_Target_Val - PID_Struct->PID_Actual_Val;      //计算偏差。
  PID_Struct->PID_Integral_Val += PID_Struct->PID_Err * PID_Struct->PID_dt;      //计算积分累加和。
  if (PID_Struct->PID_Integral_Val < PID_Integral_Min) {  //积分限幅
    PID_Struct->PID_Integral_Val = PID_Integral_Min;
  } else if (PID_Struct->PID_Integral_Val > PID_Integral_Max) {
    PID_Struct->PID_Integral_Val = PID_Integral_Max;
  }
  derivative  = (PID_Struct->PID_Err - PID_Struct->PID_Err_Last) / PID_Struct->PID_dt;    //计算微分。
  output = PID_Struct->PID_P * PID_Struct->PID_Err + PID_Struct->PID_I * PID_Struct->PID_Integral_Val + PID_Struct->PID_D * derivative;   //得到PID结果。
  if (output < PID_Output_Min) {      //PID输出结果限幅
    output = PID_Output_Min;
  } else if (output > PID_Output_Max) {
    output = PID_Output_Max;
  }
  PID_Struct->PID_Err_Last = PID_Struct->PID_Err;     //上一次偏差赋值。

  return output;
}

/**
  * @brief  PID直立环。使用PD（比例项和微分项）。
  * @param  Gyro_X：陀螺仪X轴角速度。
  * @retval output：PID直立环输出结果。
  */
float PID_Vertical(float Gyro_X)
{
  float output;     //PID直立环计算结果。
  //1.直立环计算。
  output = PID_Struct_Vertical.PID_P * (PID_Struct_Vertical.PID_Actual_Val - PID_Struct_Vertical.PID_Target_Val) + PID_Struct_Vertical.PID_D * Gyro_X;
  return output;
}

/**
  * @brief  PID速度环。使用PI（比例项和积分项）。
  * @param  None
  * @retval output：PID速度环输出结果。
  */
float PID_Velocity(void)
{
  float output;       //PID速度环计算结果。
  float ERR_LPF;      //偏差滤波结果。

  //1.计算偏差。
  PID_Struct_Velocity.PID_Err = PID_Struct_Velocity.PID_Actual_Val - PID_Struct_Velocity.PID_Target_Val;
  //2.低通滤波。
  ERR_LPF = (1-PID_Struct_Velocity.LPF) * PID_Struct_Velocity.PID_Err + PID_Struct_Velocity.LPF * PID_Struct_Velocity.PID_Err_Last;
  //4.积分累加和。
  if(PID_Struct_Velocity.PID_Target_Val != 0)
  {
    PID_Struct_Velocity.PID_Integral_Val = 0;
  } else{
    PID_Struct_Velocity.PID_Integral_Val += ERR_LPF;
    //5.积分限幅。
    PID_Struct_Velocity.PID_Integral_Val = PID_Struct_Velocity.PID_Integral_Val > 20000 ? 20000 : (PID_Struct_Velocity.PID_Integral_Val < (-20000) ? (-120000) : PID_Struct_Velocity.PID_Integral_Val);
  }
  //6.速度环计算。
  output = PID_Struct_Velocity.PID_P * ERR_LPF + PID_Struct_Velocity.PID_I * PID_Struct_Velocity.PID_Integral_Val;
  //3.保存此次误差。
  PID_Struct_Velocity.PID_Err_Last = PID_Struct_Velocity.PID_Err;

  return output;
}

/**
  * @brief  PID转向环。使用PD（比例项和微分项）。
  * @param  gyro_Z：Z轴角速度值。
  * @retval output：PID转向环输出结果。
  */
float PID_Turn(float gyro_Z)
{
  float output;

  //1.转向环计算。
  output = PID_Struct_Turn.PID_P * PID_Struct_Turn.PID_Target_Val + PID_Struct_Turn.PID_D * gyro_Z;

  return output;
}


